{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fg_intro_md"
      },
      "source": [
        "# FactorGraph"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fg_desc_md"
      },
      "source": [
        "A `FactorGraph` represents a factor graph, a bipartite graph connecting variables and factors. In GTSAM, the `FactorGraph` class (and its templated instantiations like `GaussianFactorGraph`, `NonlinearFactorGraph`, etc.) primarily stores a collection of factors.\n",
        "\n",
        "This class serves as the base for different types of factor graphs. You typically work with specific instantiations like `gtsam.GaussianFactorGraph` or `gtsam.NonlinearFactorGraph`.\n",
        "\n",
        "The total probability $P(X)$ represented by a factor graph is proportional to the product of its individual factor potentials $\\phi_i$:\n",
        "$$\n",
        "P(X) \\propto \\prod_i \\phi_i(X_i)\n",
        "$$\n",
        "where $X_i$ are the variables involved in factor $i$. In terms of error (negative log-likelihood):\n",
        "$$\n",
        "P(X) \\propto \\exp\\left(-\\sum_i \\text{error}_i(X_i)\\right)\n",
        "$$\n",
        "The total error for the graph given an assignment $X$ is the sum of the errors of the individual factors:\n",
        "$$\n",
        "\\text{error}(X) = \\sum_i \\text{error}_i(X_i)\n",
        "$$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fg_colab_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/inference/doc/FactorGraph.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "fg_pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "fg_import_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "import graphviz\n",
        "\n",
        "# Example uses NonlinearFactorGraph, but concepts apply to others\n",
        "from gtsam import NonlinearFactorGraph, PriorFactorPose2, BetweenFactorPose2, Pose2, Point3\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "X = symbol_shorthand.X"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fg_init_md"
      },
      "source": [
        "## Initialization and Adding Factors\n",
        "\n",
        "A `FactorGraph` is typically created empty and factors are added individually or from containers."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "fg_create_code",
        "outputId": "23456789-abcd-ef01-2345-6789abcdef01"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Graph size after adding factors: 2\n"
          ]
        }
      ],
      "source": [
        "graph = NonlinearFactorGraph()\n",
        "\n",
        "# Define noise models\n",
        "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))\n",
        "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.2, 0.2, 0.1))\n",
        "\n",
        "# Create factors\n",
        "factor1 = PriorFactorPose2(X(0), Pose2(0, 0, 0), prior_noise)\n",
        "factor2 = BetweenFactorPose2(X(0), X(1), Pose2(1, 0, 0), odometry_noise)\n",
        "factor3 = BetweenFactorPose2(X(1), X(2), Pose2(1, 0, 0), odometry_noise)\n",
        "\n",
        "# Add factors to the graph\n",
        "graph.add(factor1)  # add is synonym for push_back\n",
        "graph.push_back(factor2)\n",
        "\n",
        "print(f\"Graph size after adding factors: {graph.size()}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fg_access_md"
      },
      "source": [
        "## Accessing Factors and Properties"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "fg_access_code",
        "outputId": "3456789a-bcde-f012-3456-789abcdef012"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Is graph empty? False\n",
            "Number of factors (size): 2\n",
            "Number of non-null factors (nrFactors): 2\n",
            "Factor at index 1: \n",
            "BetweenFactor(x0,x1)\n",
            "  measured:  (1, 0, 0)\n",
            "  noise model: diagonal sigmas [0.2; 0.2; 0.1];\n",
            "Keys involved in the graph: x0x1\n"
          ]
        }
      ],
      "source": [
        "print(f\"Is graph empty? {graph.empty()}\")\n",
        "print(f\"Number of factors (size): {graph.size()}\")\n",
        "print(f\"Number of non-null factors (nrFactors): {graph.nrFactors()}\") # Useful if factors were removed\n",
        "\n",
        "# Access factor by index\n",
        "retrieved_factor = graph.at(1)\n",
        "print(\"Factor at index 1: \")\n",
        "retrieved_factor.print()\n",
        "\n",
        "# Get all unique keys involved in the graph\n",
        "all_keys = graph.keys() # Returns a KeySet\n",
        "print(f\"Keys involved in the graph: {all_keys}\")\n",
        "\n",
        "# Iterate through factors\n",
        "# for i, factor in enumerate(graph):\n",
        "#     if factor:\n",
        "#         print(f\"Factor {i} keys: {factor.keys()}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fg_error_md"
      },
      "source": [
        "## Graph Error\n",
        "\n",
        "The `error(Values)` method calculates the total error of the graph for a given assignment of variable values. This is the sum of the errors from each individual factor."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "fg_error_code",
        "outputId": "456789ab-cdef-0123-4567-89abcdef0123"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Total graph error at ground truth: 0.0\n",
            "Total graph error with incorrect x2: 0.0\n"
          ]
        }
      ],
      "source": [
        "values = gtsam.Values()\n",
        "values.insert(X(0), Pose2(0, 0, 0))\n",
        "values.insert(X(1), Pose2(1, 0, 0))\n",
        "values.insert(X(2), Pose2(2, 0, 0))\n",
        "\n",
        "total_error1 = graph.error(values)\n",
        "print(f\"Total graph error at ground truth: {total_error1}\")\n",
        "\n",
        "# Introduce an error\n",
        "values.update(X(2), Pose2(1, 0, 0))\n",
        "total_error2 = graph.error(values)\n",
        "print(f\"Total graph error with incorrect x2: {total_error2:.1f}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fg_viz_md"
      },
      "source": [
        "## Graph Visualization\n",
        "\n",
        "Factor graphs can be visualized using Graphviz via the `dot()` method."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "fg_dot_code",
        "outputId": "56789abc-def0-1234-5678-9abcdef01234"
      },
      "outputs": [
        {
          "data": {
            "image/svg+xml": [
              "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
              "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
              " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
              "<!-- Generated by graphviz version 2.50.0 (0)\n",
              " -->\n",
              "<!-- Pages: 1 -->\n",
              "<svg width=\"134pt\" height=\"84pt\"\n",
              " viewBox=\"0.00 0.00 134.00 83.60\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
              "<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 79.6)\">\n",
              "<polygon fill=\"white\" stroke=\"transparent\" points=\"-4,4 -4,-79.6 130,-79.6 130,4 -4,4\"/>\n",
              "<!-- var8646911284551352320 -->\n",
              "<g id=\"node1\" class=\"node\">\n",
              "<title>var8646911284551352320</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"27\" y=\"-53.9\" font-family=\"Times New Roman,serif\" font-size=\"14.00\">x0</text>\n",
              "</g>\n",
              "<!-- factor0 -->\n",
              "<g id=\"node3\" class=\"node\">\n",
              "<title>factor0</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"27\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352320&#45;&#45;factor0 -->\n",
              "<g id=\"edge1\" class=\"edge\">\n",
              "<title>var8646911284551352320&#45;&#45;factor0</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M27,-39.58C27,-25.79 27,-7.97 27,-3.73\"/>\n",
              "</g>\n",
              "<!-- factor1 -->\n",
              "<g id=\"node4\" class=\"node\">\n",
              "<title>factor1</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"74\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352320&#45;&#45;factor1 -->\n",
              "<g id=\"edge2\" class=\"edge\">\n",
              "<title>var8646911284551352320&#45;&#45;factor1</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M40.09,-41.61C52.26,-27.68 69.09,-8.41 73.11,-3.81\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352321 -->\n",
              "<g id=\"node2\" class=\"node\">\n",
              "<title>var8646911284551352321</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"99\" y=\"-53.9\" font-family=\"Times New Roman,serif\" font-size=\"14.00\">x1</text>\n",
              "</g>\n",
              "<!-- var8646911284551352321&#45;&#45;factor1 -->\n",
              "<g id=\"edge3\" class=\"edge\">\n",
              "<title>var8646911284551352321&#45;&#45;factor1</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M91.36,-40.17C84.93,-26.32 76.46,-8.1 74.44,-3.76\"/>\n",
              "</g>\n",
              "</g>\n",
              "</svg>\n"
            ],
            "text/plain": [
              "<graphviz.sources.Source at 0x17b3cbfcc20>"
            ]
          },
          "execution_count": 5,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "graphviz.Source(graph.dot(values))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fg_elim_md"
      },
      "source": [
        "## Elimination\n",
        "\n",
        "A key purpose of factor graphs is inference via variable elimination. `FactorGraph` itself doesn't perform elimination, but its derived classes (like `GaussianFactorGraph`, `SymbolicFactorGraph`) inherit `eliminateSequential` and `eliminateMultifrontal` methods from `EliminateableFactorGraph`."
      ]
    }
  ],
  "metadata": {
    "colab": {
      "provenance": []
    },
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}